//
// Created by bobin on 17-6-9.
//

#include <glob.h>
#include <vector>
#include <string>
#include <algorithm>
#include <fstream>
#include <sstream>
#include <assert.h>
#include <string>
#include "KittiReader.h"
#include <iostream>

using namespace std;


inline std::vector<std::string> glob_vector(const std::string &pat) {
    using namespace std;
    glob_t glob_result;
    glob(pat.c_str(), GLOB_TILDE, NULL, &glob_result);
    vector<string> ret;
    for (unsigned int i = 0; i < glob_result.gl_pathc; ++i) {
        ret.push_back(string(glob_result.gl_pathv[i]));
    }
    globfree(&glob_result);
    return ret;
}

bool LoadGroundTruthTraj(const string &trajPath,
                         TrajectoryType &trajectory) {

    ifstream fTraj(trajPath);
    if (!fTraj) {
        cout << "cannot find trajectory file!" << endl;
        return false;
    }

    while (!fTraj.eof()) {
        string s;
        getline(fTraj, s);
        if (!s.empty()) {
            if ((s[0] < '0' || s[0] > '9') && s[0] != '-') // not a number
                continue;

            stringstream ss;
            ss << s;
            double data[12];
            for (double &d:data) {
                ss >> d;
                if (ss.peek() == ',' || ss.peek() == ' ')
                    ss.ignore();

//                    std::cout << d << std::endl;
            }
            Eigen::Map<const Eigen::Matrix<double, 4, 3>> pose(data);
            Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
            mat.topLeftCorner<3, 4>() = pose.transpose();
            trajectory.push_back(mat);
//                LOG(INFO) << "\ngt\n" << pose.matrix() << "\nframe " << trajectory.size() << std::endl;
        }
    }

    fTraj.close();

    return true;
}

bool LoadImgNameFromPath(const std::string &strPathLeft, const std::string &strPathRight,
                         const std::string &strPathTimes, std::vector<std::string> &vstrImageLeft,
                         std::vector<std::string> &vstrImageRight, std::vector<double> &vTimeStamps) {

    vstrImageLeft = glob_vector(strPathLeft + "*.png");
    std::sort(vstrImageLeft.begin(), vstrImageLeft.end());
    vstrImageRight = glob_vector(strPathRight + "*.png");
    std::sort(vstrImageRight.begin(), vstrImageRight.end());


    std::ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    if (!fTimes) {
        cout << "cannot find timestamp file: " << strPathTimes << std::endl;
        return false;
    }

    while (!fTimes.eof()) {
        std::string s;
        getline(fTimes, s);
        if (!s.empty()) {
            std::stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimeStamps.push_back(t);
        }
    }
    assert(vstrImageLeft.size() == vstrImageRight.size());
    assert(vstrImageLeft.size() == vTimeStamps.size());


}

